#include <stdio.h>
#include "A.h"

A::A(const Location& start, const Location& end) : m_start(start), m_end(end), m_step_count(0)
{
    //ctor
}

bool A::IsPathFound() const
{
    return m_step_count > 20 || m_open_nodes.empty() || m_open_nodes.rend()->loc == m_end;
}

bool A::IsClosed(const Node& node) const
{
    printf("Test [%d,%d f:%d]\n", node.loc.row, node.loc.col, node.f);
    for (NodeSet::const_iterator x = m_close_nodes.begin(); x != m_close_nodes.end(); ++x)
    {
        printf("[%d,%d f:%d] ", x->loc.row, x->loc.col, x->f);
        if (x->loc.row == node.loc.row && x->loc.col == node.loc.col)
            return true;
    }
    return false;
    //NodeSet::const_iterator it = m_close_nodes.find(node);
    return  m_close_nodes.find(node) != m_close_nodes.end();
}

void A::find()
{
    int f = std::abs(m_start.row - m_end.row) + std::abs(m_start.col - m_end.col);
    Node start(m_start, f,0,f);
    m_open_nodes.insert(start);

    do
    {
        Node current = *m_open_nodes.begin();
        printf("Selected point [%d,%d f:%d]\n", current.loc.row, current.loc.col, current.f);
        Discover(current);
        m_close_nodes.insert(current);
        m_open_nodes.erase(m_open_nodes.begin());
    }
    while ( !IsPathFound());
}



void A::Discover(const Node& node)
{
    if (!IsClosed(node))
    {
        Discover(node, NORD);
        Discover(node, EAST);
        Discover(node, SOUTH);
        Discover(node, WEST);
    }
}

void A::Discover(const Node& parent, t_direction direction)
{
    Node result(parent);

    ++m_step_count;

    result.move(direction);
    ++result.g;
    result.h = std::abs(result.loc.row - m_end.row) + std::abs(result.loc.col - m_end.col);
    result.f = result.g + result.h;

    if ( IsValid(result))
    {
        m_open_nodes.insert(result);
        printf("Added point [%d,%d]\n", result.loc.row, result.loc.col);
        for (NodeSet::const_iterator x = m_open_nodes.begin(); x != m_open_nodes.end(); ++x)
            printf("[%d,%d f:%d] ", x->loc.row, x->loc.col, x->f);
    }
    else
    {
        m_close_nodes.insert(result);
        printf("Excluded point [%d,%d]\n", result.loc.row, result.loc.col);
        for (NodeSet::const_iterator x = m_close_nodes.begin(); x != m_close_nodes.end(); ++x)
            printf("[%d,%d f:%d] ", x->loc.row, x->loc.col, x->f);
    }
}
